// SEE END OF FILE FOR LICENSE TERMS

/****************************************************************************
CANDataProtocol

Resposible for tranlating data packed in CAN messages into data useable
by CANDataManager.c

****************************************************************************/


#include "CANRegisters.h"
#include "libCAN.h"
#include "CANDataProtocol.h"

#include <string.h>

// Transmission request flags
DATA_TX_REQUESTS	txRequests;

// A pointer to the next data to send
volatile unsigned int* 	ptxBuff	= NULL;
unsigned char  txBufferIndex	= 0;
unsigned char  maxSize			= 0;
unsigned char  type 			= CONTROL_DATA_NONE;

void send_next_data(void);


void CAN_parse_received_data(void)
{
	CxRXEIDbits rxBits;
	rxBits.W = CxRX0EID;

	unsigned char type  			= rxBits._.EID17_14;  		// Find the control data type
	unsigned char rxBufferIndex  	= rxBits._.EID13_6;			// Find the index into the data array
	unsigned char dataLength 		= CxRX0DLCbits.DLC;			// Find the size of data sent (in bytes)

	CAN_received_data(type, rxBufferIndex, dataLength, (unsigned char*) &CxRX0B1);
}


inline void requestRadioDataTransmit()
{
	txRequests._.tx_req_radio_data = 1;
	CAN_send_next_data();
};

inline void requestControlDataTransmit()
{
	txRequests._.tx_req_control_data = 1;
	CAN_send_next_data();
}

inline void requestServoDataTransmit()
{
	txRequests._.tx_req_servo_data = 1;
	CAN_send_next_data();
}

inline void requestFlagsDataTransmit()
{
	txRequests._.tx_req_flags_data = 1;
	CAN_send_next_data();
}



void CAN_send_next_data(void)
{
	if(CxTX0CONbits.TXREQ == 1)
	{
		return;
	};

	// Disable the CAN interrupt so that lower priority threads can run in safety
	CxIE = 0;
	
	if(ptxBuff == NULL)
	{
		if(txRequests._.tx_req_servo_data == 1)
		{
			ptxBuff = udb_pwOut;
			txRequests._.tx_req_servo_data = 0;
			type = CONTROL_DATA_SERVO;
			maxSize = MAX_OUTPUTS;
		}
		else if(txRequests._.tx_req_radio_data == 1)
		{
			ptxBuff = (unsigned int*) CANRadioDataBuff;
			txRequests._.tx_req_radio_data = 0;
			txBufferIndex = 0;
			type = CONTROL_DATA_RADIO;
			maxSize = 2;
		}
		else if(txRequests._.tx_req_control_data == 1)
		{
			ptxBuff = CANControlDataBuff;
			txRequests._.tx_req_control_data = 0;
			txBufferIndex = 0;
			type = CONTROL_DATA_AP_CONTROL;
			maxSize = (sizeof(CANControlDataBuff)/sizeof(unsigned int));
		}
		else if(txRequests._.tx_req_flags_data == 1)
		{
			ptxBuff = CANFlagsDataBuff;
			txRequests._.tx_req_flags_data = 0;
			txBufferIndex = 0;
			type = CONTROL_DATA_FLAGS;
			maxSize = (sizeof(CANFlagsDataBuff)/sizeof(unsigned int));
		}
	}

	if(ptxBuff != NULL)
	{
		send_next_data();
	}
	
	CxIE = 1;
}


void send_next_data(void)
{
	unsigned char dataLength = maxSize - txBufferIndex;

	if(dataLength > 4)
	{
		dataLength = 4;
	}

	CxTX0EIDbits.EID17_14 	= type;
	CxTX0EIDbits.EID13_6	= txBufferIndex;
	CxTX0DLCbits.DLC		= dataLength << 1;

	CxTX0B1 = ptxBuff[txBufferIndex];
	txBufferIndex++;
	CxTX0B2 = ptxBuff[txBufferIndex];
	txBufferIndex++;
	CxTX0B3 = ptxBuff[txBufferIndex];
	txBufferIndex++;
	CxTX0B4 = ptxBuff[txBufferIndex];
	txBufferIndex++;

	// If the end of the buffer is reached, reset it for the next transmission.
	if(txBufferIndex >= maxSize)
	{
		ptxBuff = NULL;
		txBufferIndex = 0;
	}

	CxTX0CONbits.TXREQ = 1;
}



/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
// or see this website: http://code.google.com/p/gentlenav
/****************************************************************************/
